#include "trajorientationcalculate.h"
#include "frames.hpp"
#include <QDebug>
//#include <QFileDialog>
#include <QDir>
#include <QFile>

using namespace KDL;
TrajOrientationCalculate::TrajOrientationCalculate()
{

}

int TrajOrientationCalculate::readPointFromCsv(QString pathIn,QVector<PointPro> &bufferPointQueueInOut)
{

//    QFileDialog* fd = new QFileDialog(this)；//创建打开文件对话框
//    QString fileName = fd->getOpenFileName(this,tr("Open File"),"/home",tr("Excel(*.csv)"));
//    if(fileName == "")
//          return;
    QDir dir = QDir::current();
    QFile file(dir.filePath(pathIn));
    if(!file.open(QIODevice::ReadOnly))
         qDebug()<<"OPEN FILE FAILED";
    QTextStream * out = new QTextStream(&file);//文本流
    QStringList tempOption = out->readAll().split("\n");//每行以\n区分
    PointPro tmpPoint;
    tmpPoint.positionValue.resize(6);
    for(int i = 0 ; i < tempOption.count()-2 ; i++)
    {
         QStringList tempbar = tempOption.at(i).split(",");//一行中的单元格以，区分
         for(int m=0;m<tempbar.size();m++)
         {
             tmpPoint.positionValue[m]=tempbar.at(m).toDouble();
         }
         bufferPointQueueInOut.append(tmpPoint);
    }

    file.close();//操作完成后记得关闭文件

    return 1;
}



int TrajOrientationCalculate::combineOrientation(QVector<PointPro > refOri,
                                                 QVector<PointPro> &bufferPointQueueInOut)
{
    int skipPointCount=15;//间隔固定点,算斜率
    double angleBias=M_PI*20/180.0;//最小斜率偏差
    double minDistanceBias=0;//最小距离偏差
    double maxDistanceBias=0;//最大距离偏差

    QVector<QVector<double>> orientList;
    QVector<double> tmpOri;
    tmpOri.resize(3);
    tmpOri[0]=refOri[0].positionValue[3];
    tmpOri[1]=refOri[0].positionValue[4];
    tmpOri[2]=refOri[0].positionValue[5];
    orientList.append(tmpOri);
    tmpOri[0]=refOri[1].positionValue[3];
    tmpOri[1]=refOri[1].positionValue[4];
    tmpOri[2]=refOri[1].positionValue[5];
    orientList.append(tmpOri);
    tmpOri[0]=refOri[2].positionValue[3];
    tmpOri[1]=refOri[2].positionValue[4];
    tmpOri[2]=refOri[2].positionValue[5];
    orientList.append(tmpOri);

    qDebug()<<"processPointsData_v　剖口: shreshold angleBias"<<angleBias<<"minDistanceBias"<<minDistanceBias
           <<"maxDistanceBias"<<maxDistanceBias<<"skipPointCount"<<skipPointCount;

    QVector<int> keyIndex;
    //左侧计算斜率
    int featureIndex1;
    bool  isFindPoint1=false;
    int tmpCount=bufferPointQueueInOut.size()-skipPointCount*2;
    for(int i=0;i<tmpCount;i++)
    {

        double tmpAngleDiff1=calAngle(bufferPointQueueInOut[i].positionValue,
                                      bufferPointQueueInOut[i+skipPointCount].positionValue,
                bufferPointQueueInOut[i+skipPointCount*2].positionValue);
        qDebug()<<"i"<<i<<"tmpAngleDiff1"<<tmpAngleDiff1/M_PI*180.0;
        if(fabs(tmpAngleDiff1)>angleBias)
        {


            isFindPoint1=true;
            featureIndex1=i+skipPointCount;
            keyIndex.append(featureIndex1);
            qDebug()<<"左拐点"<<featureIndex1;
            i=featureIndex1;
        }

    }

    if(false==isFindPoint1)
    {

        qDebug()<<"左侧没有合格拐点";

        return false;
    }
    int tmpIndex=0;
    int oldIndex=0;
    keyIndex.append(bufferPointQueueInOut.size()-1);
    for(int m=0;m<keyIndex.size();m++)
    {
        for(int n=oldIndex;n<=keyIndex[m];n++)
        {
            switch(tmpIndex)
            {
            case 0:
                bufferPointQueueInOut[n].positionValue[3]=orientList[0][0];
                bufferPointQueueInOut[n].positionValue[4]=orientList[0][1];
                bufferPointQueueInOut[n].positionValue[5]=orientList[0][2];
                break;
            case 1:
                bufferPointQueueInOut[n].positionValue[3]=orientList[1][0];
                bufferPointQueueInOut[n].positionValue[4]=orientList[1][1];
                bufferPointQueueInOut[n].positionValue[5]=orientList[1][2];
                break;
            case 2:
                bufferPointQueueInOut[n].positionValue[3]=orientList[2][0];
                bufferPointQueueInOut[n].positionValue[4]=orientList[2][1];
                bufferPointQueueInOut[n].positionValue[5]=orientList[2][2];
                break;
            }

        }
        oldIndex=keyIndex[m];
        tmpIndex++;
        if(tmpIndex>2)
        {
            tmpIndex=0;
        }

    }

    return true;

}

double TrajOrientationCalculate::calAngle(std::vector<double> p1, std::vector<double> p2, std::vector<double> p3)
{
    Vector v1(p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]);
    Vector v2(p3[0]-p2[0],p3[1]-p2[1],p3[2]-p2[2]);
    v1.Normalize();
    v2.Normalize();
//    qDebug()<<dot(v1,v2);
    return  acos( dot(v1,v2) );//中间点的夹角
}
